function new_state = motion_model(state, control, dt, Q)
% 机器人运动模型
% 输入: state-[x;y;theta], control-[v;w], dt-时间步长, Q-噪声协方差
% 输出: new_state-新状态

    x = state(1);
    y = state(2);
    theta = state(3);
    v = control(1);
    omega = control(2);
    
    % 运动模型
    if abs(omega) < 1e-6
        x_new = x + v * dt * cos(theta);
        y_new = y + v * dt * sin(theta);
        theta_new = theta;
    else
        x_new = x + (v/omega) * (sin(theta + omega*dt) - sin(theta));
        y_new = y + (v/omega) * (-cos(theta + omega*dt) + cos(theta));
        theta_new = theta + omega * dt;
    end
    
    % 添加噪声
    if nargin > 3 && ~isempty(Q)
        noise = mvnrnd([0; 0; 0], Q)';
        new_state = [x_new; y_new; theta_new] + noise;
    else
        new_state = [x_new; y_new; theta_new];
    end
    
    new_state(3) = wrapToPi(new_state(3));
end

